/**
 * @file ROS_CAN_LC.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief LCCAN 数据透传节点，订阅 CAN_send_data 话题，发布 CAN_receive_data 话题
 * @version 2.8
 * @date 2020-07-08
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "ros_can_driver.h"

#include "ros/ros.h"
#include "ros_car/VCU_frame.h"

/**
 * @brief CAN 控制指令监听回调函数
 * 
 * @param msg CAN_send_data 话题消息
 */
void SendROSDatatoCAN(const ros_car::VCU_frame::ConstPtr& msg);

/**
 * @brief CAN 数据透传节点
 * 
 * @param argc 
 * @param argv 
 * @return int 
 */
int main(int argc, char **argv)
{
  setlocale(LC_ALL, "");
  ros::init(argc, argv, "ROS_CAN");

  ROS_INFO("This is ROS_CAN");

  ros::NodeHandle CAN;
  ros::Subscriber receive = CAN.subscribe("CAN_send_data", 1000, SendROSDatatoCAN);
  ros::Publisher send = CAN.advertise<ros_car::VCU_frame>("CAN_receive_data", 1000);
  
  ros::Rate loop_rate(DEF_CAR_MESSAGE_FREQ);
  
  ros_car::VCU_frame send_msg;
  CAN_DataFrame rec;
  CAN_ErrorInformation err;
  
  if(0 == LCCANInit())
  {
      return 0;
  }

  if(0 == LCCANChanleStart())
  {
      return 0;
  }

  while (ros::ok())
  {
    if ( (CAN_ChannelReceive(g_devicehandle, DEF_CAN_CHANNEL_SELECT, &rec, sizeof(rec), 200)) > 0 )
    {
      send_msg.uTimeFlag = rec.uTimeFlag;
      send_msg.uID = rec.uID;
      send_msg.nSendType = rec.nSendType;
      send_msg.nDataLen = rec.nDataLen;
      send_msg.bRemoteFlag = rec.bRemoteFlag;
      send_msg.bExternFlag = rec.bExternFlag;

      for(int i = 0; i < 8; ++ i)
      {
        send_msg.arryData[i] = rec.arryData[i];
      }
      send.publish(send_msg);
        
    } else 
    {
      if ( CAN_GetErrorInfo(g_devicehandle, DEF_CAN_CHANNEL_SELECT, &err) == CAN_RESULT_OK ) // 必须调用CAN_GetErrorInfo函数
      {
        // 处理错误信息
      } else {
        // CAN卡没有收到CAN报文
      }
    }
    ros::spinOnce();
    loop_rate.sleep();
  }

  return 0;
}

void SendROSDatatoCAN(const ros_car::VCU_frame::ConstPtr& msg)
{
  CAN_DataFrame tmp;
  tmp.uTimeFlag = msg->uTimeFlag;
  tmp.uID = msg->uID;
  tmp.nSendType = msg->nSendType;
  tmp.nDataLen = msg->nDataLen;
  tmp.bRemoteFlag = msg->bRemoteFlag;
  tmp.bExternFlag = msg->bExternFlag;

  for(int i = 0; i < 8; ++ i)
  {
    tmp.arryData[i] = msg->arryData[i];
  }
  // vcu_frame_data_format_out(&tmp);
  Send(&tmp);
}